
#include "Map.h"
#include "World.h"
#include "PowerUp/Spawner.h"
#include "PowerUp/SpeedPowerUp.h"
#include "Utils/Vector2D.h"
#include "Flag.h"
#include "GameState.h"
#include "AI/RRT.h"
#include "Settings.h"

#include <cassert>
#include <GL/gl.h>
#include <iostream>
#include <SDL/SDL_image.h>
#include <vector>
#include <cmath>


struct PixelColour {
    unsigned char r, g, b;
};



Map::Map() {
    float map_width = Settings::instance().getFloatValue("map", "width");
    float map_height = Settings::instance().getFloatValue("map", "height");

    min_coord = Vector2D(-map_width/2.0f, -map_height/2.0f);
    max_coord = Vector2D(map_width/2.0f, map_height/2.0f);
}

Map::~Map(){

}

bool Map::load(std::string filename){
    SDL_Surface *map_def = IMG_Load(filename.c_str());
    if(!map_def){
        std::cerr << "Invalid map file: " << filename << std::endl;  
        return false;
    }

    if(map_def->format->BytesPerPixel != 3){
        std::cerr << "Invalid map file format: " << filename << std::endl;  
        return false;
    }

    std::vector<PixelColour> map_image;
    map_image.reserve(map_def->w*map_def->h);

    for(int i = 0; i < map_def->w*map_def->h; i++){
        PixelColour np;
        np.r = ((unsigned char *)map_def->pixels)[i*3];
        np.g = ((unsigned char *)map_def->pixels)[i*3+1];
        np.b = ((unsigned char *)map_def->pixels)[i*3+2];
        map_image.push_back(np);
    }

    loadMapObstacles(map_image, map_def->w, map_def->h);
    loadEntities(map_image, map_def->w, map_def->h);

    return true;
}

void Map::render(void){
    renderBackground();
    renderMapObstacles();
}

void Map::renderMinimap(void){
    renderBackgroundMinimap();
    renderMapObstaclesMinimap();
}

bool Map::collideRaySegment(Vector2D start, Vector2D end, Vector2D &normal, 
                            Vector2D &collision_point, bool use_collision_space, 
                            bool tight_collision_space){

    Vector2D closest_collision_point;
    Vector2D closest_normal;
    float closest_dist = 0.0f;
    bool result = false;

    for(unsigned i = 0; i < all_obstacles.size(); i++){
        Vector2D cp, n;
        if(all_obstacles[i].collideRaySegment(start, end, n, cp, use_collision_space, 
                                              tight_collision_space)){
            
            float dist = (cp - start).length();
            if(!result || dist < closest_dist){
                closest_dist = dist;
                closest_normal = n;
                closest_collision_point = cp;
            }
 
            result = true;
        }
    }

    if(!result && !pointInsideMap(end)){
        float cur_frac = 0.5f, cur_inc = 0.25f;
        for(unsigned i = 0; i < 16; i++){
            if(!pointInsideMap(start + cur_frac*(end-start))){
                cur_frac -= cur_inc;
            }
            else{
                cur_frac += cur_inc;
            }
            cur_inc *= 0.5f;
        }

        collision_point = start + cur_frac*(end-start);
        return true;
    }


    if(result){
        normal = closest_normal;
        collision_point = closest_collision_point;
    }

    return result;
}


bool Map::collideSphere(Vector2D centre, float radius, Vector2D &normal, Vector2D &collision_point,
                        Vector2D &constraint_free_point){
    
    // We assume that the given sphere will only collide with at most 1 obstacle 
    for(unsigned i = 0; i < all_obstacles.size(); i++){
        if(all_obstacles[i].collideSphere(centre, radius, normal, collision_point, constraint_free_point)){
            return true;
        }
    }
    return false;
}

bool Map::pointInsideObstacle(Vector2D point, bool use_collision_space, bool tight_collision_space){
    for(unsigned i = 0; i < all_obstacles.size(); i++){
        if(all_obstacles[i].pointInside(point, use_collision_space, tight_collision_space)){
            return true;
        }
    }
    return false;
}

bool Map::pointInsideMap(Vector2D point){
    return point.x >= min_coord.x && point.y >= min_coord.y && point.x <= max_coord.x && point.y <= max_coord.y;
}

bool Map::clipPointInsideMap(Vector2D &point, Vector2D &cnormal){
    bool result = false;
    if(point.x < min_coord.x){ 
        point.x = min_coord.x;
        cnormal = Vector2D(1.0f, 0.0f);
        result = true;
    }
    if(point.y < min_coord.y){ 
        point.y = min_coord.y;
        cnormal = Vector2D(0.0f, 1.0f);
        result = true;
    }
    if(point.x > max_coord.x){ 
        point.x = max_coord.x;
        cnormal = Vector2D(-1.0f, 0.0f);
        result = true;
    }
    if(point.y > max_coord.y){
        point.y = max_coord.y;
        cnormal = Vector2D(0.0f, -1.0f);
        result = true;
    }
    return result;
}

float Map::getWidth(void){
    return max_coord.x - min_coord.x;
}

float Map::getHeight(void){
    return max_coord.y - min_coord.y;
}

Vector2D Map::getMinCoord(void){
    return min_coord;
}

Vector2D Map::getMaxCoord(void){
    return max_coord;
}

void Map::renderBackground(void){
    glPushMatrix();

    float intensity = Settings::instance().getFloatValue("map", "background_line_intensity");
    glColor3f(intensity, intensity, intensity);

    glLineWidth(1.0f);
    // Render the background grid
    glBegin(GL_LINES);

    unsigned num_background_lines = (unsigned)Settings::instance().getIntValue("map", "num_background_lines");
    for(unsigned i = 0; i <= num_background_lines; i++){
        glVertex3f(min_coord.x, min_coord.y + (float)i/num_background_lines*(max_coord.y-min_coord.y), 0.0f);
        glVertex3f(max_coord.x, min_coord.y + (float)i/num_background_lines*(max_coord.y-min_coord.y), 0.0f);

        glVertex3f(min_coord.x + (float)i/num_background_lines*(max_coord.x-min_coord.x), min_coord.y, 0.0f);
        glVertex3f(min_coord.x + (float)i/num_background_lines*(max_coord.x-min_coord.x), max_coord.y, 0.0f);
    }

    glEnd();
    glPopMatrix();
}


void Map::renderBackgroundMinimap(void){
    glPushMatrix();
    glColor3f(0.0f, 1.0f, 0.0f);

    // Render the background grid
    glBegin(GL_LINES);

    for(unsigned i = 0; i <= 1; i++){
        glVertex3f(min_coord.x, min_coord.y + (float)i*(max_coord.y-min_coord.y), 0.0f);
        glVertex3f(max_coord.x, min_coord.y + (float)i*(max_coord.y-min_coord.y), 0.0f);

        glVertex3f(min_coord.x + (float)i*(max_coord.x-min_coord.x), min_coord.y, 0.0f);
        glVertex3f(min_coord.x + (float)i*(max_coord.x-min_coord.x), max_coord.y, 0.0f);
    }

    glEnd();
    glPopMatrix();
}


void Map::renderMapObstacles(void){
    for(unsigned i = 0; i < all_obstacles.size(); i++){
        all_obstacles[i].render();
    }
}

void Map::renderMapObstaclesMinimap(void){
    for(unsigned i = 0; i < all_obstacles.size(); i++){
        all_obstacles[i].render();
    }
}



void Map::loadEntities(std::vector<PixelColour> map_image, int width, int height){
    for(int y = 0; y < height; y++){
        for(int x = 0; x < width; x++){
            if(isFlagColour(map_image[x + y*width])){
                unsigned team_id = GameState::instance().teamIdFromPixelColour(map_image[x + y*width].r,
                                        map_image[x + y*width].g, map_image[x + y*width].b);

                Flag *new_flag = new Flag(team_id, pixelToPosition(x, y, width, height));
                World::instance().addObject(new_flag);

                Team &team(GameState::instance().getTeam(team_id));
                team.setFlag(new_flag);
            }
        }
    }
}

bool Map::isFlagColour(const PixelColour &col){
    return (col.r == 255 && col.g == 0 && col.b == 0) ||
           (col.r == 0 && col.g == 0 && col.b == 255);
}

void Map::loadMapObstacles(std::vector<PixelColour> map_image, int width, int height){
    assert(map_image.size() == (unsigned)(width*height));
    
    std::vector<int> obstacle_map;
    for(unsigned i = 0; i < map_image.size(); i++){
        if(isPixelObstacle(map_image[i])){
            obstacle_map.push_back(1);
        }
        else{
            obstacle_map.push_back(0);
        }
    }

    int current_label = 2;
    for(int y = 0; y < height; y++){
        for(int x = 0; x < width; x++){
            if(obstacle_map[x + y*width] == 1){
                fillObstacleRegion(obstacle_map, width, height, current_label, x, y);
                current_label++;
            }
        }
    }


    float ship_size_tight = Settings::instance().getFloatValue("ship", "size")/sqrtf(2.0f) - 0.05f;
    float ship_size_loose = Settings::instance().getFloatValue("ship", "size");
    for(int label = 2; label < current_label; label++){
        MapObstacle new_obstacle(min_coord, max_coord, ship_size_tight, ship_size_loose);
        new_obstacle.load(obstacle_map, width, height, label);
        all_obstacles.push_back(new_obstacle);
    }
}

void Map::fillObstacleRegion(std::vector<int> &obstacle_map, int width, int height, int label, int startx, int starty){
    std::list<int> coordx, coordy;
    coordx.push_front(startx);
    coordy.push_front(starty);
    unsigned int filled = 0;

    while(!coordx.empty() && !coordy.empty()){
        int cx = coordx.front(); coordx.pop_front();
        int cy = coordy.front(); coordy.pop_front();

        if(cx < 0 || cx >= width || cy < 0 || cy >= height){ continue; }
        if(obstacle_map[cx + cy*width] != 1){ continue; }

        obstacle_map[cx + cy*width] = label;
        filled++;
        coordx.push_front(cx+1); coordy.push_front(cy);
        coordx.push_front(cx-1); coordy.push_front(cy);
        coordx.push_front(cx); coordy.push_front(cy+1);
        coordx.push_front(cx); coordy.push_front(cy-1);
    }
}

bool Map::isPixelObstacle(const PixelColour &pixel){
    return pixel.r == 0 && pixel.g == 0 && pixel.b == 0; // if the pixel is black
}

Vector2D Map::pixelToPosition(int pixel_x, int pixel_y, int width, int height){
    float xfrac = (float)pixel_x/(float)width;
    float yfrac = (float)pixel_y/(float)height;

    float xwidth = max_coord.x - min_coord.x;
    float ywidth = max_coord.y - min_coord.y;

    return Vector2D(xfrac*xwidth + min_coord.x, yfrac*ywidth + min_coord.y);
}

